function [t,qt,v1]=planeTrajectoryThirdOrder(t0,t1,q0,q1,v0,v1,step)
h=q1-q0, T=t1-t0;
a0=q0;
a1=v0;
a2=(3*h-(2*v0+v1)*T)/T^2;
a3=(-2*h+(v0+v1)*T)/T^3;

t = linspace(t0,t1,step);
qt = a0 + a1.*(t-t0) + a2.*(t-t0).^2 + a3.*(t-t0).^3;
%{
dqt_ = diff(qt); dqt = cat(2,0,dqt_);
ddqt_ = diff(dqt); ddqt = cat(2,0,ddqt_);
subplot(311),plot(t,qt);grid on;
subplot(312),plot(t,dqt);grid on;
subplot(313),plot(t,ddqt);grid on;
%}
plot(t,qt);grid on;
end